#ifndef G2O_PNP_VERTEX_H_
#define G2O_PNP_VERTEX_H_

#include <Eigen/Core>
#include <sophus/se3.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>

// vertex used in g2o ba
// template parameters
//    1. 6: the size of the update
//    2. Sophus::SE3d is the type of _estimate
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override
    {
        // the pose to be optimized
        _estimate = Sophus::SE3d();
    }

    // apply the delta to the estimation, such that
    //     _esimate += _delta;
    virtual void oplusImpl(const double *update) override
    {
        Eigen::Matrix<double, 6, 1> update_eigen;
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];

        // left multiplication on SE3
        _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
    }

    virtual bool read(istream &in) override { return false; }
    virtual bool write(ostream &out) const override { return false; }
};

#endif
